package testApplets;

import java.awt.Color;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;
import java.awt.geom.Point2D;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintStream;
import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.Scanner;
import java.util.Vector;

import org.w3c.dom.Document;
import org.w3c.dom.Element;
import org.w3c.dom.NodeList;

import marchingCubes.DualCube;
import marchingCubes.IsMCVertex;
import marchingCubes.MCube;
import marchingCubes.MCubeEngineLowMemory;

import controlP5.ControlP5;
import controlP5.Range;
import controlP5.Slider;
import peasy.PeasyCam;
import processing.core.PApplet;
import processing.core.PGraphics;
import processing.core.PGraphics2D;
import processing.core.PGraphics3D;
import processing.core.PMatrix3D;
import simpleGeom.AbstractFace;
import simpleGeom.BasicMesh;
import simpleGeom.Bounds3D;
import simpleGeom.Box3D;
import simpleGeom.DepthAnalyser3D;
import simpleGeom.Face3D;
import simpleGeom.HasPos3D;
import simpleGeom.PCamera3D;
import simpleGeom.PGraphicsHelper;
import simpleGeom.Plane;
import simpleGeom.Polygon2D;
import simpleGeom.QuadFace;
import simpleGeom.Rect2D;
import simpleGeom.SVGLoader;
import simpleGeom.SimpleTriangle;
import simpleGeom.TrianglePreCalc;
import simpleGeom.Vec3D;
import testApplets.MeshToSolid2.Cell;

public class FinalMeshCutApplet extends PApplet {
	String path = "/Users/caad/Desktop/Final Shape/";
	String meshPath = path +"g_scaledFixed.obj" ;//"final5kLevel6.obj"
	String outlinePath=path+"GrottoOutline7.svg";
	BasicMesh mesh;
	Vector<SimpleTriangle> meFaces;
	PCamera3D cam;
	ControlP5 cp5;
	

	float sX, sY, sZ;
	float dim = 0.005f;//0.0051f;//0.0035
	float dimX = dim;
	float dimY = dim;
	float dimZ = dim;

	int fac = 16;
	Cell[][][] grid;
	
	Box3D bounds;
	// bounds: x1:-2.14084 y1:-2.22545 z1:-1.7528 dX:4.28168 dY:4.13662
	// dZ:4.46073

	Range xRange, yRange, zRange;


	int x1, y1, z1, x2, y2, z2;
	int nX, nY, nZ;


	Box3D innerBounds;
	Color[] colors = new Color[] { Color.CYAN, Color.GREEN, Color.RED,
			Color.BLUE, Color.YELLOW, Color.MAGENTA, Color.ORANGE,
			Color.DARK_GRAY };


	
	float iso = dim *0.9f;

	boolean doReset;
	boolean doSave;
	float maxD = 10;
	int step = 10;
	int drawY=660;//605
	
	boolean doInit=false;
	Polygon2D polyOut;
	//setting halfshape
	//Box3D cB=new  Box3D(0.04f,-1.6f,-1.325f,2f,1.5f,0.1f);
	//almost working Box3D cB=new  Box3D(0.13f,-1.3f,-1.4f,2f,1.5f,0.0f);
	//Box3D cB=new  Box3D(0.15f,-1.55f,-1.32f,2f,1.45f,0.0f);
	//Box3D cB=new  Box3D(0.17f,-1.55f,-1.50f,1.8f,1.5f,0.0f);
	//Box3D cB=new  Box3D(0.06f,-1.6f,-1.30f,1.7f,1.5f,0.0f);
	
	//Box3D cB=new Box3D(0f,0f,-1.75f,2.0f,3.2f,0f);// this is the working version
	//Box3D cB=new Box3D(0f,0f,-1.75f,1.6f,3.2f,0f);//unscaled small
	//Box3D cB=new Box3D(0f,0f,-1.6f,1.5f,3.1f,0f);
	Box3D cB=new Box3D(0.00f,0f,-1.52f,1.5f,3.25f,0f);
	// unscaled Box3D cB=new  Box3D(0.06f,-1.6f,-1.30f,1.7f,1.5f,0.0f);
	//Box3D cB=new  Box3D(-0.05f,-1.27f,-1.30f,1.7f,1.5f,0.0f);

	int nReached=0;
	boolean hasOutline;
	ArrayList<AbstractFace>clippedFaces=new ArrayList<AbstractFace>();
	float[][]outlineGrid;
	float[] yCor=new float[]{0.0f,0.4f,0.8f,1.6f,2.4f,2.8f,3.2f}; 
	float[] xCor; 
	float[] zCor; 
	Rect2D[]cRects=new Rect2D[4];
	
	public void setup() {
		
		cRects[0]=new Rect2D(0,-1.75f,1.2f,-1.2f);
		cRects[1]=new Rect2D(0,-1.2f,1.2f,0f);
		
		cRects[2]=new Rect2D(1.2f,-1.75f,2,-0.55f);
		cRects[3]=new Rect2D(1.2f,-0.55f,2,0);
		cRects=new Rect2D[16];
		float dX=1.6f/4;
		float dZ=1.75f/4;
		int cI=0;
		xCor=new float[5];
		zCor=new float[5];
		
		for (int x=0;x<5;x++){
			xCor[x]=x*dX;
		}
		for (int z=0;z<5;z++){
			zCor[z]=z*dZ-1.75f;
		}
		for (int x=0;x<4;x++){
			for (int z=0;z<4;z++){
				Rect2D rect=new Rect2D(x*dX,z*dZ-1.75f,(x+1)*dX,(z+1)*dZ-1.75f);
				cRects[cI]=rect;
				cI++;
			}
		}
		
		size(2000, 1200, OPENGL);
		cam = new PCamera3D(this, 0, 0, 220);
		//saveBoxes();
		if (doInit){
			mesh = new BasicMesh();
			mesh.load(meshPath);
		    /*mesh.translate(0, 2.01f, .27f);
			mesh.scale(.857f, .884f, .884f);
			mesh.saveObj(meshPath+"scaled.obj");*/
			bounds=mesh.getBounds3D();
			bounds=bounds.getOffsetBox(0.2f);
			//bounds.z2=0.1f;
			createGrid();
			calculateDistances();
			saveDistance(meshPath + "iii.txt");
		}
		else{
			loadDistance(meshPath + "iii.txt");
			createGUI();
			reset();
		}
		//loadOutline(outlinePath);
		// 
		// exportFaces(this.selectOutput(),gridFine);
	}
	
	
	
	
	
	
	public void findClippedFaces(float y){
		clippedFaces=new ArrayList<AbstractFace>();
		for (int i=0;i<mesh.faces.size();i++){
			AbstractFace face=mesh.faces.get(i);
			int nUp=0;
			int nDown=0;
			for (int j=0;j<face.getNodesNum();j++){
				HasPos3D n=face.getNode(j);
				if (n.getY()>y){
					nUp++;
				}
				else{
					nDown++;
				}
			}
			if (nUp>0&&nDown>0){
				clippedFaces.add(face);
			}
		}
	}
	public void loadOutline(String file){
		SVGLoader loader=new SVGLoader();
		Document svgDoc=loader.loadDoc(new File(file));
		NodeList list=svgDoc.getElementsByTagName("polygon");
		Element svgPoly=(Element) list.item(0);
		Vector<Point2D>pts=loader.getPolygon(svgPoly);
		polyOut=new Polygon2D();
		for (int i=0;i<pts.size();i++){
			Point2D pt=pts.get(i);
			Vec3D v=new Vec3D((float)pt.getX(),(float)pt.getY());
			polyOut.addPt(v);
		}
		setOutlineDist();
		/*String[] strings=this.loadStrings(file);
		
		String first=strings[0];
		String[] coords=first.split(" ");
		for (int i=0;i<coords.length;i+=2){
			float xC=Float.valueOf(coords[i]);
			float yC=Float.valueOf(coords[i+1]);
			Vec3D v=new Vec3D(xC,yC);
			polyOut.addPt(v);
		}*/
		//polyOut.reverse();
		
	}
	public void setOutlineDist(){
		outlineGrid=new float[nX][nZ];
		for (int x=0;x<nX;x++){
			for (int z=0;z<nZ;z++){
				float cX=getX(x);
				float cZ=getZ(z);
				float cD=polyOut.getClosestDist(cX, cZ);
				outlineGrid[x][z]=cD;
				if (!polyOut.contains(cX, cZ)){
					outlineGrid[x][z]*=-1;
				}
			}
		}
		/*for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (!cell.reached && cell.getValue() > iso) {
						cell.setValue(0);
					}
				}
			}
		}*/
	}
	
	public void reset() {
		// PrintWriter output = createWriter(meshPath + "H" + x1 + "_" +
		// x2+"_"+y1+"_"+y2+"_"+z1+"_"+z2+ "inside" + dim + "res005.obj");
		x1 = xK(xRange.getLowValue());
		x2=xK(xRange.getHighValue());
		//x2 = nX - 1 - x1;
		x2=nX-1;
		y1 = yK(yRange.getLowValue());
		y2 = yK(yRange.getHighValue()) - 1;
		//y2=drawY;
		z1 = zK(zRange.getLowValue());
		z2 = zK(zRange.getHighValue()) - 1;
		
		
		innerBounds=new Box3D(xRange.getLowValue(),yRange.getLowValue(),zRange.getLowValue(),bounds.x2-(xRange.getLowValue()-bounds.x1),yRange.getHighValue(),zRange.getHighValue());
		
		innerBounds=new Box3D(cB);
		
		x1=xK(cB.x1);
		y1=yK(cB.y1);
		z1=zK(cB.z1);
		x2=xK(cB.x2);
		y2=yK(cB.y2);
		z2=zK(cB.z2);
		
		println("find Outline");
		//findOutline((int) (nX * 0.5f), (int) (nY * 0.5f), (int) (nZ * 0.5f));
		outlineGrid=new float[nX][nZ];
		
		//findOutline((int) nX-1,(int)( (y1+y2)*0.5f),z1+1);
		findOutline(this.xK(0.3f),this.yK(1),this.zK(-0.2f));
		println("make Border");
		makeBorder();
		println("make Isofaces");
		//findClippedFaces(drawY* dimY + sY);
		 meFaces = MCubeEngineLowMemory.initCubesTest(grid, iso, null);
		
	}
	public void saveBoxes(){
		try {
			FileWriter fw;
			fw = new FileWriter(new File(meshPath + "buildUpBox.obj"));
			BufferedWriter bw = new BufferedWriter(fw);
			int iVertices = 0;
			for (int y = 0; y < yCor.length - 1; y++) {
				for (int x = 0; x < xCor.length - 1; x++) {
					for (int z = 0; z < zCor.length - 1; z++) {
						bw.write("g " + iVertices);
						bw.newLine();
						Box3D cBox = new Box3D(xCor[x], yCor[y], zCor[z],
								xCor[x + 1], yCor[y + 1], zCor[z + 1]);
						iVertices=cBox.saveObj(bw, iVertices);
					}
				}
			}
			println("end save");
			bw.flush(); // Writes the remaining data to the file
			bw.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	public void saveSegments() {
		int iN = 1;
		String faceString = "";
		
		try {
			FileWriter fw;
			fw = new FileWriter(new File(meshPath + "buildUp.obj"));
			BufferedWriter bw = new BufferedWriter(fw);
			int cBoxes = 0;
			for (int y = 0; y < yCor.length - 1; y++) {
				for (int x = 0; x < xCor.length - 1; x++) {
					for (int z = 0; z < zCor.length - 1; z++) {

						// for (int i=0;i<cRects.length;i++){
						restoreDist();
						// Rect2D cRect=cRects[i];
						// Box3D cBox=new
						// Box3D(cRect.x1,yCor[y],cRect.y1,cRect.x2,yCor[y+1],cRect.y2);
						Box3D cBox = new Box3D(xCor[x], yCor[y], zCor[z],
								xCor[x + 1], yCor[y + 1], zCor[z + 1]);
						makeBorder(cBox);

						println("start save");
						meFaces = MCubeEngineLowMemory.initCubesTest(grid, iso,
								null);
						bw.write("g " + cBoxes);
						bw.newLine();
						cBoxes++;
						for (int j = 0; j < meFaces.size(); j++) {
							SimpleTriangle f = meFaces.get(j);
							for (int jj = 0; jj < 3; jj++) {
								HasPos3D p = f.getNode(jj);
								p.setId(-1);
							}
						}
						for (int j = 0; j < meFaces.size(); j++) {
							SimpleTriangle f = meFaces.get(j);
							for (int jj = 0; jj < 3; jj++) {
								HasPos3D p = f.getNode(jj);
								if (p.getId() < 0) {
									p.setId(iN);
									bw.write("v " + p.getX() + " " + p.getY()
											+ " " + p.getZ());
									bw.newLine();
									iN++;
								}
							}
						}
						for (int j = 0; j < meFaces.size(); j++) {
							SimpleTriangle f = meFaces.get(j);
							faceString = "f";
							for (int jj = 0; jj < 3; jj++) {
								HasPos3D p = f.getNode(jj);
								faceString += " " + p.getId();
							}
							bw.write(faceString);
							bw.newLine();
						}

					}
				}

			}
			println("end save");
			bw.flush(); // Writes the remaining data to the file
			bw.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	public void saveObj(){
		
		//adjustIsoInner();
		//PrintWriter output = createWriter(meshPath +"Innerd"+dim+"iso"+iso+"x1"+x1+"y1"+y1+"z1"+z1+"x2"+x2+"y2"+y2+"z2"+z2+".obj");
		PrintWriter output = createWriter(meshPath +"Innerd"+dim+"iso"+iso+cB.toString()+".obj");
		println("start save");
		meFaces=MCubeEngineLowMemory.initCubesTest(grid, iso, output);
		println("end save");
		output.flush(); // Writes the remaining data to the file
		output.close();

	}
	
	public void createGrid() {
		nX = (int) (bounds.getDimX() / dimX) + 1;
		nY = (int) (bounds.getDimY() / dimY) + 1;
		nZ = (int) (bounds.getDimZ() / dimZ) + 1;
		sX = bounds.getX1();
		sY = bounds.getY1();
		sZ = bounds.getZ1();
		x1 = 0;
		y1 = 0;
		z1 = 0;
		x2 = nX;
		y2 = nY;
		z2 = nZ;
		grid = new Cell[nX][nY][nZ];
		for (int x = 0; x < nX; x++) {
			println("x: " + x + "/" + nX);
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = new Cell(x, y, z);
					cell.setValue(maxD);
					grid[x][y][z] = cell;
				}
			}
		}
	}

	public void makeBorder() {
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (!cell.reached
							&& (x <= x1 || x >= x2 || y <= y1 || y >= y2 || z <= z1 || z >= z2)) {
						cell.setValue(maxD);
					}
				}
			}
		}
	}
	public void makeBorder(Box3D box) {
		int x1=xK(box.x1);
		int y1=yK(box.y1);
		int z1=zK(box.z1);
		int x2=xK(box.x2);
		int y2=yK(box.y2);
		int z2=zK(box.z2);
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (!cell.reached && (x <= x1 || x >= x2 || y <= y1 || y >= y2 || z <= z1 || z >= z2)) {
						cell.setValue(maxD);
					}
				}
			}
		}
	}


	public void createGUI() {
		cp5 = new ControlP5(this);
		// .setRange(bounds.x2,bounds.x2)
		xRange = cp5.addRange("xRange").setPosition(20, 50).setBroadcast(false)
				.setSize(200, 20).setHandleSize(20)
				.setRange(bounds.x1, bounds.x2)
				.setRangeValues(cB.x1,cB.x2).setBroadcast(true);
		// xRange.snapToTickMarks(false);
		// xRange.s
		// xRange.setLowValue(-1.95f);
		println("lowValue: " + xRange.getLowValue());
		yRange = cp5.addRange("yRange").setPosition(20, 80).setBroadcast(false)
				.setSize(200, 20).setHandleSize(20)
				.setRange(bounds.y1, bounds.y2)
				.setRangeValues(cB.y1,cB.y2).setBroadcast(true);
		zRange = cp5.addRange("zRange").setPosition(20, 110)
				.setBroadcast(false).setSize(200, 20).setHandleSize(20)
				.setRange(bounds.z1, bounds.z2)
				.setRangeValues(cB.z1,cB.z2).setBroadcast(true);
		/*
		 * sliderX1=cp5.addSlider("x1").setPosition(20,20 ).setRange(bounds.x1,
		 * bounds.x2).setSize(200, 20).setValue(bounds.x1);
		 * //sliderX2=cp5.addSlider("x2").setPosition(20,
		 * 40).setRange(bounds.x1, bounds.x2) .setSize(200,
		 * 20).setValue(bounds.x2);
		 * 
		 * sliderY1=cp5.addSlider("y1").setPosition(20,40).setRange(bounds.y1,
		 * bounds.y2) .setSize(200, 20).setValue(bounds.y1);
		 * sliderY2=cp5.addSlider("y2").setPosition(20, 60).setRange(bounds.y1,
		 * bounds.y2) .setSize(200, 20).setValue(bounds.y2);
		 * 
		 * sliderZ1=cp5.addSlider("z1").setPosition(20, 80).setRange(bounds.z1,
		 * bounds.z2) .setSize(200, 20).setValue(bounds.z1);
		 * sliderZ2=cp5.addSlider("z2").setPosition(20, 100).setRange(bounds.z1,
		 * bounds.z2) .setSize(200, 20).setValue(bounds.z2);
		 */

		cp5.setAutoDraw(false);
	}
public void drawYPlane(int i){
	findClippedFaces(drawY* dimY + sY);
	float goalY= 0;
	float eyeY= goalY+200;
	
	camera(0,eyeY,0,0,goalY,0,0,0,1);
	
	//
	float cX=cB.getX1()+(cB.getX2()-cB.getX1())*0.3f;
	float cZ=cB.getZ1()+(cB.getZ2()-cB.getZ1())*0.3f;
	float sc=150;
	translate(-cX*sc,0,-cZ*sc);
	stroke(0,255,0);
	scale(sc);
	
	//pushMatrix();
	//translate(sX,sY,sZ);
	//scale(dimX,dimY,dimZ);
	//;
	/*Cell cCell=grid[140][606][100];
	stroke(255,255,0);
	line(cCell.getX(),0,cCell.getZ(),cCell.getX(),0,cCell.getZ()+dimZ*5);
	while(cCell.prev!=null){
		stroke(255,0,255);
		if (cCell.y==i){
		line(cCell.getX(),0,cCell.getZ(),cCell.prev.getX(),0,cCell.prev.getZ());
		}
		cCell=cCell.prev;
	}*/
	colorMode(HSB,255);
	for (int x = x1; x < x2; x ++) {
			for (int z = z1; z < z2; z ++) {
				/*Cell cell=grid[x][i][z];
				Cell prev=cell.prev;
				if (prev!=null){
					stroke((cell.id*1f/nReached)*255,255,255);
					line(cell.getX(),0,cell.getZ(),prev.getX(),0,prev.getZ());
				}*/
				/*noStroke();
				fill(255);
				if (cell.getValue()<iso){
					pushMatrix();
					translate(cell.getX(),0,cell.getZ());
					box(dimX);
					popMatrix();
				}*/
			}
	}
	colorMode(RGB,255);
	//popMatrix();
	stroke(255);
	fill(255,0,0);
	noFill();
	//println("clippedFaces: "+clippedFaces.size());
	beginShape(TRIANGLES);
	for (int j=0;j<clippedFaces.size();j++){
		AbstractFace face=clippedFaces.get(j);
		
		for (int jj=0;jj<3;jj++){
			HasPos3D v=face.getNode(jj);
			vertex(v.getX(),0,v.getZ());
		}
		
		//face.display3D(g);
	}
	endShape();
}


	public void draw() {
		if (mouseX < 200) {
			cam.setActive(false);
		} else {
			cam.setActive(true);
		}
		if (doReset) {
			reset();
			doReset = false;
		}
		if (doSave) {
			saveObj();
			//saveSegments();
			doSave = false;
		}
		
		//
		// innerBounds=new
		// Box3D(sliderX1.getValue(),sliderY1.getValue(),sliderZ1.getValue(),bounds.x2-sliderX1.getValue(),sliderY2.getValue(),sliderZ2.getValue());
		display();
		stroke(0, 255, 0);
		noFill();
		display2D();
		// noLoop();
	}

	public void drawCell(Cell cell) {
		float cX = cell.getX();
		float cY = cell.getY();
		float cZ = cell.getZ();
		pushMatrix();
		translate(cX, cY, cZ);
		box(dimX * step, dimY * step, dimZ * step);
		popMatrix();
	}

	public void displayGrid() {
		fill(255, 0, 0);
		int cI=0;
		int cStep=step*2;
		for (int x = x1; x < x2; x ++) {
			for (int y = y1; y < y2; y ++) {
				for (int z = z1; z < z2; z ++) {
					Cell cell = grid[x][y][z];
					if (x==x1||x==x2-1||y==y1||y==y2-1||z==z1||z==z2-1){
						if (!cell.reached) {
							cI++;
							if (cI%cStep==0){
							drawCell(cell);
							}
						}
					}
				}
			}
		}
		fill(255);
		
		Cell[] nbs = new Cell[3];
		for (int x = 0; x < nX - step; x += step) {
			for (int y = 0; y < nY - step; y += step) {
				for (int z = 0; z < nZ - step; z += step) {
					Cell cell = grid[x][y][z];
					nbs[0] = grid[x + 1][y][z];
					nbs[1] = grid[x][y + 1][z];
					nbs[2] = grid[x][y][z + 1];
					boolean doDraw = false;
					for (int i = 0; i < nbs.length; i++) {
						Cell nb = nbs[i];
						// if (cell.d<iso!=nb.d<iso){
						if (cell.reached != nb.reached) {
							doDraw = true;
						}
					}
					//if (doDraw) {
						if (cell.getValue()<iso){
						drawCell(cell);
						//}
					}
				}
			}
		}
		colorMode(HSB,255);
		strokeWeight(2f);
		for (int x = 0; x < nX - step; x += step) {
			for (int y = 0; y < nY - step; y += step) {
				for (int z = 0; z < nZ - step; z += step) {
					Cell cell = grid[x][y][z];
					if (cell.reached){
						stroke((cell.id*1f/nReached)*255,255,255);
						pushMatrix();
						float cX = cell.getX();
						float cY = cell.getY();
						float cZ = cell.getZ();
						
						point(cX,cY,cZ);
						popMatrix();
						
					}
				}
			}
		}
		colorMode(RGB,255);
		noStroke();
	}

	public void display() {
		background(0);
		// lights();
		rotateY(HALF_PI-0.7f);
		pointLight(220, 210, 200, 2, -3, 1);
		pointLight(220, 220, 230, -2, 2, 2f);
		 PGraphicsHelper.drawWorldAxis3D(g, 1);
		//translate(-bounds.getCenterX(),-bounds.getCenterY(),-bounds.getCenterZ());
		
		rotateX(PI);
		translate(0,-1.6f,0);
		
		
		fill(255);
		noStroke();
		
		if (innerBounds!=null){
			noFill();
			stroke(255,0,0);
			innerBounds.display3D(g);
		}
		noStroke();
		
		if (mesh != null) {
				fill(0,255,0);
			for (int i = 0; i < mesh.faces.size(); i += 50) {
				AbstractFace face = mesh.faces.get(i);
				// face.display3D(g);
			}
		}
		if (meFaces!=null){
			//println("meFaces.size(): "+meFaces.size());
			fill(255);
		for (int i=0;i<meFaces.size();i++){
			AbstractFace face = meFaces.get(i);
			face.display3D(g);
		}
		}
		stroke(0,255,0);
		//drawYPlane(drawY);
		//displayGrid();

	}

	public void display2D() {
		hint(DISABLE_DEPTH_TEST);
		colorMode(RGB, 255);
		noLights();
		stroke(0);
		fill(0);
		camera();
		cp5.draw();
		/*
		 * for (int i=0;i<xButs.length;i++){ Button b=xButs[i]; b.display(g); }
		 * for (int i=0;i<yButs.length;i++){ Button b=yButs[i]; b.display(g); }
		 * for (int i=0;i<zButs.length;i++){ Button b=zButs[i]; b.display(g); }
		 * bgroup.display(g); groupType.display(g); int iY=bgroup.getSelected();
		 * fill(255,0,0);
		 */
		hint(ENABLE_DEPTH_TEST);
	}
	public boolean contains(Cell cell){
		if (polyOut==null)return true;
		if (outlineGrid[cell.x][cell.z]>=0&&cell.y>y1&&cell.y<y2){
			return true;
		}
		return false;
	}
	public void calculateDistances() {
		// Box3D box3D = b3d.getOffsetBox(dim*4f+iso);
		TrianglePreCalc tri;
		for (int i = 0; i < mesh.faces.size(); i++) {
			if (i % 100000 == 0) {
				println(i + "/" + mesh.faces.size());
			}
			SimpleTriangle f = (SimpleTriangle) mesh.faces.get(i);
			tri = new TrianglePreCalc(f.getNode(0), f.getNode(1), f.getNode(2));
			Box3D faceBounds = tri.getBounds3D();
			faceBounds = faceBounds.getOffsetBox(0.025f);//iso
			int x1 = xK(faceBounds.x1) - 1;
			int y1 = yK(faceBounds.y1) - 1;
			int z1 = zK(faceBounds.z1) - 1;
			x1 = max(x1, 0);
			y1 = max(y1, 0);
			z1 = max(z1, 0);
			int x2 = xK(faceBounds.x2) + 2;
			int y2 = yK(faceBounds.y2) + 2;
			int z2 = zK(faceBounds.z2) + 2;
			x2 = min(x2, nX);
			y2 = min(y2, nY);
			z2 = min(z2, nZ);
			for (int x = x1; x < x2; x++) {
				for (int y = y1; y < y2; y++) {
					for (int z = z1; z < z2; z++) {
						// float minD = Float.MAX_VALUE;
						Cell cell = grid[x][y][z];
						float cellX = cell.getX();
						float cellY = cell.getY();
						float cellZ = cell.getZ();
						float planeDist = tri.getDistancePlane(cellX, cellY,
								cellZ);
						if (planeDist < cell.getValue()) {
							float cDist = Float.MAX_VALUE;
							cDist = tri.getDistance(cellX, cellY, cellZ);
							if (cDist < cell.getValue()) {
								cell.setValue(cDist);
								//cell.normal = tri.n;
							}
						}

					}
				}
			}
		}
	}
	float[][][]backupDist;
	void backupDist(){
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					backupDist[x][y][z]=cell.d;
				}
			}
		}
	}
	void restoreDist(){
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					cell.d=backupDist[x][y][z];
				}
			}
		}
	}
	public void findOutline(int cX, int cY, int cZ) {
		
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					cell.reached = false;
					cell.prev=null;
					cell.id=0;
				}
			}
		}
		
		Vector<Cell> candidates = new Vector<Cell>();
		candidates.add(grid[cX][cY][cZ]);
		for (int i = 0; i < candidates.size(); i++) {
			Cell cell = candidates.get(i);
			cell.reached = true;
		}

		while (candidates.size() > 0) {
			Vector<Cell> nextCandidates = new Vector<Cell>();
			for (int i = 0; i < candidates.size(); i++) {
				Cell cell = candidates.get(i);

				int sX = max(cell.getXK() - 1, x1);
				int sY = max(cell.getYK() - 1, y1);
				int sZ = max(cell.getZK() - 1, z1);

				int eX = min(cell.getXK() + 2, x2);
				int eY = min(cell.getYK() + 2, y2);
				int eZ = min(cell.getZK() + 2, z2);

				for (int x = sX; x < eX; x++) {
					for (int y = sY; y < eY; y++) {
						for (int z = sZ; z < eZ; z++) {
							Cell nb = grid[x][y][z];
							//if (contains(nb)){
							if (!nb.reached&&nb.getValue() > iso) {
								nb.reached = true;
								nb.id=nReached;
								nReached++;
								nb.prev=cell;
								nextCandidates.add(nb);
							//}
							}
						}
					}
				}
			}
			candidates = nextCandidates;
		}
		adjustIso();
	}
	
	public void adjustIsoInner() {
		iso = 0.006f;// wallthickness;
		// reached and bigger iso=outside
		// calculating the distance to the reached area , if its bigger iso turn
		// it to a big number=clipping the inner structures
		float[][][] distances=new float[nX][nY][nZ];
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (cell.reached){
					distances[x][y][z]=0;
					}
					else{
						distances[x][y][z]=10000;
					}
				}
			}
		}
		DepthAnalyser3D dA=new DepthAnalyser3D();
		dA.calculateDepth(distances, dimX, dimX*sqrt(2), dimX*sqrt(3));
		distances=dA.depth;
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if ((distances[x][y][z]>iso+dimX*4f)&&cell.getValue()<iso){
						cell.setValue(distances[x][y][z]);
					}
				}
			}
		}
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (cell.reached && cell.getValue() > iso) {
						cell.setValue(0);
					}
				}
			}
		}
		
		
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					float cX = cell.getX();
					float cY = cell.getY();
					float cZ = cell.getZ();

					float cOut = outlineGrid[x][z];
					// outside und nicht erreicht
					if (!cell.reached
							&& (cOut < 0 || y < y1 || y >= y2 || z < z1)) {
						cell.setValue(0);
					}
					// innen und nicht erreicht.
					
					if (!cell.reached && contains(cell)) {
						if (cell.getValue() > cOut) {
							cell.setValue(cOut);
						}
						float minD = cB.getInnerDistance(cX, cY, cZ);
						if (cell.getValue() > minD) {
							cell.setValue(minD);
						} //
					}
					 //cell.setValue(min(cOut,cell.getValue())); }
				}
			}
		}
		
		// now the stripes
		// every 20cm 1cm
	
		ArrayList<Plane>planes=new ArrayList<Plane>();
		int nX=(int)(cB.getDimX()/0.2f)+1;
		float dX=cB.getDimX()/nX;
		println("sX: "+cB.getX1());
		for (int iX=1;iX<nX;iX++){
			float cX=iX*dX+cB.getX1();
			println("cX: "+cX);
			Vec3D v=new Vec3D(cX,0,0);
			Plane plane=new Plane(v,new Vec3D(1,0,0));
			planes.add(plane);
		}
		int nY=(int)(cB.getDimY()/0.2f)+1;
		float dY=cB.getDimY()/nY;
		for (int iY=1;iY<nY;iY++){
			float cY=iY*dY+cB.getY1();
			Vec3D v=new Vec3D(0,cY,0);
			Plane plane=new Plane(v,new Vec3D(0,1,0));
			planes.add(plane);
		}
		int nZ=(int)(cB.getDimZ()/0.2f)+1;
		float dZ=cB.getDimZ()/nZ;
		for (int iZ=1;iZ<nZ;iZ++){
			float cZ=iZ*dZ+cB.getZ1();
			Vec3D v=new Vec3D(0,0,cZ);
			Plane plane=new Plane(v,new Vec3D(0,0,1));
			planes.add(plane);
		}
	//	println("planes.size(): "+planes.size());
		for (int x = 0; x < grid.length; x++) {
			for (int y = 0; y < grid[0].length; y++) {
				for (int z = 0; z < grid[0][0].length; z++) {
					Cell cell = grid[x][y][z];
					Vec3D cP=new Vec3D(cell.getX(),cell.getY(),cell.getZ());
					
					float minD=10000;
					if (!cell.reached) {
						for (int i=0;i<planes.size();i++){
							Plane plane=planes.get(i);
							float cD=abs(plane.getDistanceSigned(cP));
						
							if (cD<minD)minD=cD;
							
						}
						//println("minD: "+minD);
						if (cell.getValue()>minD){
							cell.setValue(minD);
						}
					}
				}
			}
		}

	}
	
	public void adjustIso(){
		//
		float[][][] distances=new float[nX][nY][nZ];
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					distances[x][y][z]=cell.getValue();
				}
			}
		}
		// not reached and bigger iso=inside
		for (int x = 0; x < nX; x++) {
			for (int y = 0; y < nY; y++) {
				for (int z = 0; z < nZ; z++) {
					Cell cell = grid[x][y][z];
					if (!cell.reached && cell.getValue() > iso) {
						cell.setValue(0);
					}
				}
			}
		}
			if (polyOut!=null){
				println("has Outline");
				for (int x = 0; x < nX; x++) {
					for (int y = 0; y < nY; y++) {
						for (int z = 0; z < nZ; z++) {
							Cell cell = grid[x][y][z];
							float cX=cell.getX();
							float cY=cell.getZ();
							float cOut=outlineGrid[x][z];
							//outside und nicht erreicht
							if (cOut<0&&!cell.reached){
								cell.setValue(abs(cOut));
							}
							//inside und nicht erreicht
							
							if (( y <y1 || y >=y2||z<z1)){
								cell.setValue(maxD);
							}
						}
					}
				}
			}
			backupDist();
		
	}
	public int roundUp(int n, int a) {
		if (n % a == 0)
			return n;
		return ((int) (n / a) + 1) * a;
	}

	public void loadDistance(String file) {
		nX = 0;
		nY = 0;
		nZ = 0;
		int cX = 0;
		int cY = 0;
		int cZ = 0;
		try {
			BufferedReader in = new BufferedReader(new FileReader(file));
			int iLine = 0;
			String cString = null;
			while ((cString = in.readLine()) != null) {

				if (iLine == 0) {
					String[] strings = cString.split(" ");
					nX = Integer.valueOf(strings[0]);
					nY = Integer.valueOf(strings[1]);
					nZ = Integer.valueOf(strings[2]);
					x1 = 0;
					y1 = 0;
					z1 = 0;
					x2 = nX;
					y2 = nY;
					z2 = nZ;
					println("nX: " + nX);
					println("nY: " + nY);
					println("nZ: " + nZ);
					
				} else if (iLine == 1) {
					String[] strings = cString.split(" ");
					float x1 = Float.valueOf(strings[0]);
					float y1 = Float.valueOf(strings[1]);
					float z1 = Float.valueOf(strings[2]);
					float x2 = Float.valueOf(strings[3]);
					float y2 = Float.valueOf(strings[4]);
					float z2 = Float.valueOf(strings[5]);
					sX = x1;
					sY = y1;
					sZ = z1;
					bounds = new Box3D(x1, y1, z1, x2, y2, z2);
					dimX = bounds.getDimX() * 1f / nX;
					dimY = bounds.getDimY() * 1f / nY;
					dimZ = bounds.getDimZ() * 1f / nZ;
					println("dimX: " + dimX);
					println("dimY: " + dimY);
					println("dimZ: " + dimZ);
					dim = dimX;
					grid = new Cell[nX][nY][nZ];
				} else {
					int j = iLine - 2;

					// int cX = ((int) (j * 1f / (nZ * nY)));
					// int cY = (int) (j * 1f / nZ) % nY;
					// int cZ = j % (nZ);
					float d = Float.valueOf(cString);
					Cell cell = new Cell(cX, cY, cZ);
					cell.setValue(d);
					grid[cX][cY][cZ] = cell;
					cZ++;
					if (cZ == nZ) {
						cZ = 0;
						cY++;
						if (cY == nY) {
							cY = 0;
							cX++;
							println("cX: " + cX);
						}
					}
				}

				iLine++;
			}
		} catch (FileNotFoundException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (NumberFormatException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		backupDist=new float[nX][nY][nZ];
		backupDist();
		println("loaded");
		println("bounds: "+bounds);
	}

	public String getRound(float number, int digits) {
		return "" + (Math.round(number * digits) *1f/ digits);
	}

	public void saveDistanceSlow(String file) {
		try {
			FileOutputStream fileOutputStream = new FileOutputStream(file);
			PrintStream printstream = new PrintStream(fileOutputStream);
			printstream.println(grid.length + " " + grid[0].length + " "
					+ grid[0][0].length);
			printstream.println(bounds.x1 + " " + bounds.y1 + " " + bounds.z1
					+ " " + bounds.x2 + " " + bounds.y2 + " " + bounds.z2);
			for (int x = 0; x < grid.length; x++) {
				println("x" + x + "/" + grid.length);
				for (int y = 0; y < grid[x].length; y++) {
					for (int z = 0; z < grid[x][y].length; z++) {
						Cell c = grid[x][y][z];
						printstream.println(getRound(c.getValue(), 10000));
					}
				}
			}
			printstream.close();
			fileOutputStream.close();
		} catch (Exception e) {
			System.err.println("error writing file");
		}
		println("distance saved");
	}
	public void saveDistance(String file) {
		try {
			//FileOutputStream fileOutputStream = new FileOutputStream(file);
			FileWriter fw = new FileWriter(file);
			BufferedWriter bw = new BufferedWriter(fw);
			//PrintStream printstream = new PrintStream(fileOutputStream);
			bw.write(grid.length + " " + grid[0].length + " "
					+ grid[0][0].length);
			bw.newLine();
			bw.write(bounds.x1 + " " + bounds.y1 + " " + bounds.z1
					+ " " + bounds.x2 + " " + bounds.y2 + " " + bounds.z2);
			bw.newLine();
			for (int x = 0; x < grid.length; x++) {
				println("x" + x + "/" + grid.length);
				for (int y = 0; y < grid[x].length; y++) {
					for (int z = 0; z < grid[x][y].length; z++) {
						Cell c = grid[x][y][z];
						if (c.getValue()==maxD){
							bw.write("1");
						}
						else{bw.write(getRound(c.getValue(), 100000));}
						bw.newLine();
					}
				}
			}
			bw.flush();
			bw.close();
			//fileOutputStream.close();
		} catch (Exception e) {
			System.err.println("error writing file");
		}
		println("distance saved");
	}

	public int xK(float x) {
		return (int) ((x - sX) / dimX);
	}

	public int yK(float y) {
		return (int) ((y - sY) / dimY);
	}

	public int zK(float z) {
		return (int) ((z - sZ) / dimZ);
	}
	public float getX(int x) {
		// TODO Auto-generated method stub
		// return nX%3;
		return x * dimX + sX;
		// return x;
	}

	
	public float getY(int y) {
		// TODO Auto-generated method stub
		return y * dimY + sY;
		// return y;
	}

	public float getZ(int z) {
		// TODO Auto-generated method stub
		return z * dimZ + sZ;
		// return z;
	}

	public void keyPressed() {
		if (key == 'r') {
			doReset = true;
		}
		if (key == 's') {
			doSave = true;
		}
		if (key=='y'){
			drawY++;
			if (drawY>=nY)drawY=0;
			println("y: "+drawY);
		}
		if (key=='a'){
			drawY--;
			if (drawY<0)drawY=nX-1;
			println("y: "+drawY);
		}
	}

	class ButtonGroup {
		ArrayList<Button> buttons = new ArrayList<Button>();
		boolean radio;

		public void add(Button btn) {
			btn.parent = this;
			buttons.add(btn);
		}

		public void display(PGraphics g) {
			for (int i = 0; i < buttons.size(); i++) {
				Button button = buttons.get(i);
				button.display(g);
			}
		}

		public int getSelected() {
			for (int i = 0; i < buttons.size(); i++) {
				Button btn = buttons.get(i);
				if (btn.isActive())
					return i;
			}
			return 0;
		}

		public void setRadioActive(Button button) {
			if (radio) {
				for (int i = 0; i < buttons.size(); i++) {
					buttons.get(i).active = false;
					if (buttons.get(i) == button) {
						buttons.get(i).active = true;
					}
				}
			} else {
				button.active = true;
			}
		}
	}

	class Button extends Rect2D implements MouseListener {

		ButtonGroup parent;
		Color color = Color.LIGHT_GRAY;
		int id;

		public Button(float x1, float y1, float x2, float y2) {
			super(x1, y1, x2, y2);
			// TODO Auto-generated constructor stub
		}

		public void display(PGraphics g) {
			noStroke();
			if (active)
				fill(255, 0, 0);
			else
				fill(255);
			super.display(g);
			fill(color.getRGB());
			rect(x1 + 2, y1 + 2, this.getDimX() - 4, this.getDimY() - 4);
		}

		boolean active;

		public boolean isActive() {
			return active;
		}

		public void setActive(boolean active) {
			this.active = active;
		}

		@Override
		public void mouseClicked(MouseEvent arg0) {
			// TODO Auto-generated method stub

			if (contains(arg0.getX(), arg0.getY())) {
				if (parent != null && parent.radio) {
					parent.setRadioActive(this);
				} else {
					active = !active;
				}
			}
		}

		@Override
		public void mouseEntered(MouseEvent arg0) {
			// TODO Auto-generated method stub

		}

		@Override
		public void mouseExited(MouseEvent arg0) {
			// TODO Auto-generated method stub

		}

		@Override
		public void mousePressed(MouseEvent arg0) {
			// TODO Auto-generated method stub

		}

		@Override
		public void mouseReleased(MouseEvent arg0) {
			// TODO Auto-generated method stub
		}
	}

	class Cell implements IsMCVertex {
		short x, y, z;
		Cell prev;
		float d = Float.MAX_VALUE;
		//Vec3D normal;
		boolean reached;
		int id;
		MCube cube;

		Cell(int x, int y, int z) {
			this.x = (short) x;
			this.y = (short) y;
			this.z = (short) z;
		}

		public int getXK() {
			return x;
		}

		public int getYK() {
			return y;
		}

		public int getZK() {
			return z;
		}

		@Override
		public float getX() {
			// TODO Auto-generated method stub
			// return nX%3;
			return x * dimX + sX;
			// return x;
		}

		@Override
		public float getY() {
			// TODO Auto-generated method stub
			return y * dimY + sY;
			// return y;
		}

		public float getZ() {
			// TODO Auto-generated method stub
			return z * dimZ + sZ;
			// return z;
		}

		public float getValue() {
			return d;
		}

		public void setValue(float value) {
			this.d = value;
		}

		@Override
		public void setPosition(float x, float y, float z) {
			// TODO Auto-generated method stub

		}

		@Override
		public void setPosition(float x, float y) {
			// TODO Auto-generated method stub

		}

		@Override
		public int getId() {
			// TODO Auto-generated method stub
			return 0;
		}

		@Override
		public void setId(int i) {
			// TODO Auto-generated method stub

		}

		@Override
		public MCube getCube() {
			return cube;
		}

		@Override
		public void setCube(MCube cube) {
			// TODO Auto-generated method stub
			this.cube=cube;
		}

		@Override
		public void setZ(float z) {
			// TODO Auto-generated method stub

		}

		@Override
		public void setX(float x) {
			// TODO Auto-generated method stub

		}

		@Override
		public void setY(float Y) {
			// TODO Auto-generated method stub

		}

	}

}
